#!/usr/bin/env python 
# -*- coding: utf-8 -*-
 
import rospy #导入rospy库
import actionlib #导入actionlib 库
import os,inspect #导入os库
from actionlib_msgs.msg import * #导入actionlib的所有模块
from geometry_msgs.msg import Pose, Point, Quaternion, Twist #导入四个消息数据类型，姿态，目标点，四元数，运动消息Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal #导入movebase的两个消息数据类型
from tf.transformations import quaternion_from_euler #导入tf变换库的欧拉角转四元数库
from aikit_arm.srv import aikit_claw_srv
from math import pi,isnan #导入圆周率pi
from std_msgs.msg import String #导入标准消息的字符串消息数据格式
from sensor_msgs.msg import RegionOfInterest
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import numpy as np
import yaml,time
nav_goals=[]
goal_dict=dict()
depth_array = None
cvb=CvBridge()

# result=""
place_name=""

qrcode_data=""
object_data=""
text_data=""

qrcode_roi=RegionOfInterest()
object_roi=RegionOfInterest()
text_roi=RegionOfInterest()

x_need=280
distance_need=0.20
x_error=5
distance_error=0.05

kt=-0.0028
bt=0
km=0.2
bm=0

close_to_flag=0

qrcode_xpos=0
qrcode_distance=0.

object_xpos=0
object_distance=0

text_pose=0
text_distance=0

qrcode_data_list=["人工","智能"]
text_data_list=["机器人","教育"]
object_data_list=["bottle","pottedplant","bicycle","cat"]

#初始化节点
rospy.init_node('nav_goal_place',anonymous=False)
square_size = 1.0
cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) #实例化一个消息发布函数
move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) #action服务器连接

#设置参数 
rospy.loginfo('等待move_base action服务器连接...')
move_base.wait_for_server(rospy.Duration(30))
rospy.loginfo('已连接导航服务')

rospy.wait_for_service("aikit_claw")
rospy.loginfo('已连接机械臂控制服务')
claw = rospy.ServiceProxy("aikit_claw", aikit_claw_srv)

yaml_path = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) + "/goal2.yaml"
#添加导航坐标点,输入x（前）坐标，y（左）坐标，th（平面朝向0～360度）
def nav_to(x,y,th):
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id='map'
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose = pose_e(x,y,th)
    move(goal)

#写一个函数 用于任务完成提示。
def move(goal):
    move_base.send_goal(goal)
    finished_within_time = move_base.wait_for_result(rospy.Duration(40))
    if not finished_within_time:
        move_base.cancel_goal()
        rospy.loginfo('时间超时，导航任务取消。')
    state = move_base.get_state()
    if state == GoalStatus.SUCCEEDED:
        rospy.loginfo(place_name+'导航成功！')
    
def shutdown():
    rospy.loginfo('机器人任务停止')
    move_base.cancel_goal()
    rospy.sleep(2)
    cmd_vel_pub.publish(Twist)
    rospy.sleep(1)
def stop_move():
    t=Twist()
    cmd_vel_pub.publish(t)
 
def pose_e(x,y,th):#输入x（前）坐标，y（左）坐标，th（平面朝向0～360度）
    new_pose=Pose()
    new_pose.position.x=float(x)
    new_pose.position.y=float(y)
    #机器朝向，平面朝向弧度转化成四元数空间位姿
    q=quaternion_from_euler(0.0,0.0,float(th)/180.0*pi)
    new_pose.orientation.x=q[0]
    new_pose.orientation.y=q[1]
    new_pose.orientation.z=q[2]
    new_pose.orientation.w=q[3]
    return  new_pose

def yaml_read():
    global goal_dict
    global nav_goals
    f = open(yaml_path) 
    cfg =  f.read()
    goal_dict = yaml.load(cfg)
    nav_goals=goal_dict.keys()

# def nav_callback(data):
#     g=data.data
#     # print(type(g))
#     goal=g.decode("utf-8")
#     # print(type(goal))
#     if goal in nav_goals:
#         goal_data=goal_dict[goal]
#         nav_to(goal_data[0],goal_data[1],goal_data[2])

def nav_place(place):
    global place_name
    place_name=place
    print("尝试导航去:"+place_name)
    goal=place.decode("utf-8")
    if goal in nav_goals:
        goal_data=goal_dict[goal]
        nav_to(goal_data[0],goal_data[1],goal_data[2])

def play(filepath):
    cmd="play ~/Desktop/wrc_demo/mp3/"+filepath+".mp3"
    os.system(cmd)
   
# def nav_input():
#     rospy.Subscriber("nav_topic",String,nav_callback,queue_size=1)
#     rospy.spin()

def qrcode_callback(data):
    global qrcode_data
    qrcode_data=data.data
    # print(qrcode_data)

def object_callback(data):
    global object_data
    object_data=data.data
    # print(object_data)
    
# def text_callback(data):
#     global text_data
#     text_data=data.data
#     # print(object_data)

def qrcode_roi(data):
    global qrcode_xpos
    global qrcode_distance
    qrcode_xpos=roi_to_xpos(data)
    qrcode_distance=roi_to_distance(data)

def object_roi(data):
    global object_xpos
    global object_distance
    object_xpos=roi_to_xpos(data)
    object_distance=roi_to_distance(data)
    print(object_distance)

# def text_roi(data):
#     global text_xpos
#     global text_distance
#     text_xpos=roi_to_xpos(data)
#     text_distance=roi_to_distance(data)
#     print(text_distance)

def convert_depth_image(ros_image):
    global depth_array
    depth_image = cvb.imgmsg_to_cv2(ros_image, "passthrough")
    depth_array = np.array(depth_image, dtype=np.float32)

def msg_sub():
    rospy.Subscriber("/object_data",String,object_callback,queue_size=1)
    rospy.Subscriber("/qrcode_data",String,qrcode_callback,queue_size=1)
    #rospy.Subscriber("/text_data",String,text_callback,queue_size=1)
    rospy.Subscriber("/object_roi",RegionOfInterest,object_roi,queue_size=5)
    rospy.Subscriber("/qrcode_roi",RegionOfInterest,qrcode_roi,queue_size=5)
    #rospy.Subscriber("/text_roi",RegionOfInterest,object_roi,queue_size=5)
    rospy.Subscriber("/camera/depth/image", Image, convert_depth_image, queue_size=1)

def roi_to_xpos(rdata):
    x=rdata.x_offset
    width=rdata.width
    x_pos=x+width/2
    return x_pos
def roi_to_distance(rdata):
    min_x=rdata.x_offset
    max_x=rdata.x_offset+rdata.width
    min_y=rdata.y_offset
    max_y=rdata.y_offset+rdata.height
    sum_z=0
    n_z=0
    for x in range(min_x,max_x):
        for y in range(min_y,max_y):
            try:
                z=depth_array[y, x]
                if isnan(z):
                    continue
            except:
                z=0
            sum_z = sum_z + z
            n_z += 1
    if n_z:
        mean_z=float(sum_z)/n_z

        return mean_z
    else:
        return 0

def close_to_cube(cube_info):
    global close_to_flag
    # global qrcode_xpos
    # global object_xpos
    # global text_xpos
    # global qrcode_distance
    # global object_distance
    # global text_distance
    cube_type=""
    # roi=RegionOfInterest()
    
    if cube_info in qrcode_data_list:
        cube_type="qrcode"
    if cube_info in object_data_list:
        cube_type="object"
    if cube_type in text_data_list:
        cube_type="text"
    distance=0
    x_pos=0
    if cube_type=="qrcode":
        x_pos=qrcode_xpos
        distance=qrcode_distance
    if cube_type=="object":
        x_pos=object_xpos
        distance=object_distance
    if cube_info=="text":
        x_pos=qrcode_xpos
        distance=object_distance
    print("xpos:",x_pos)
    print("distance",distance)
    if x_pos>0 and distance>0:
        if abs(distance-distance_need)>distance_error or abs(x_pos-x_need)>x_error:
            turn_speed=kt*(x_pos-x_need)+bt
            if abs(turn_speed)>1.0 and turn_speed != 0:
                turn_speed=1.0*abs(turn_speed)/turn_speed
            if turn_speed<0.1 and turn_speed != 0:
                turn_speed=0.1*abs(turn_speed)/turn_speed
            print("转向速度：")
            print(turn_speed)
            move_speed=km*(distance-distance_need)+bm
            if abs(move_speed)<0.05 and move_speed != 0:
                move_speed=0.05*abs(move_speed)/move_speed
            if abs(move_speed)>0.1 and move_speed !=0:
                move_speed=0.1*abs(move_speed)/move_speed


            print("前后速度：")
            print(move_speed)
            twist_data=Twist()
            twist_data.linear.x=move_speed
            twist_data.angular.z=turn_speed
            cmd_vel_pub.publish(twist_data)
            close_to_flag=0
        else:
            close_to_flag=1
    else:
        twist_data=Twist()
        cmd_vel_pub.publish(twist_data)


def wait_for_result():
    result=""
    clear_data()
    while True:
        if qrcode_data in qrcode_data_list:
            result=qrcode_data
            break
        elif object_data in object_data_list:
            result=object_data
            break
        elif text_data in text_data_list:
            result=text_data
            break
    return result

def take_cube(pos):
    claw(10)
    rospy.sleep(3)
    claw(pos)
    play("getcube")

def put_cube():
    play("arrive投放框")
    claw(10)
    play("finishcube")

def clear_data():
    global text_data
    global object_data
    global qrcode_data
    text_data=""
    object_data=""
    qrcode_data=""
def move_for(xspeed,tspeed,time_second):
    twist_data=Twist()
    twist_data.linear.x=xspeed
    twist_data.angular.z=tspeed
    time_start=time.time()
    while time.time()-time_start<time_second:
        cmd_vel_pub.publish(twist_data)
    cmd_vel_pub.publish(Twist())
def close_to(name):
    print("开始瞄准:"+name)
    time_s=time.time()
    while close_to_flag==0 and time.time()-time_s<4:
        close_to_cube("智能")
    stop_move()
    print("瞄准完成")
    move_for(0.15,0,3)
def try_to(place_name,pos):
    nav_place(place_name)
    play("arrive识别区")
    name=wait_for_result()
    play("resultis")
    play(name)
    close_to(name)
    take_cube(pos)
    move_for(-0.1,0,2)
    nav_place(name)
    put_cube()
    move_for(-0.1,0,2)

if __name__ == "__main__":
    yaml_read()
    msg_sub()
    claw(10)
    play("start")
    try_to("实物1",100)
    try_to("条码1",80)
    nav_place("启动区")
    # # name=wait_for_result()
    # # play("resultis")
    # # play(name)
    # # close_to(name)

    rospy.spin()
    



       

        







